/**************************************************************************************************
* Title: SmurfsRigidBody.cpp
* Author: Gael Huber
* Description: Holds a rigid body used for physics and collision systems.
**************************************************************************************************/
#include "SmurfsRigidBody.h"

/**************************************************************************************************
* Constructor
**************************************************************************************************/
RigidBody::RigidBody(void) {
	center = Vector3();
	velocity = Vector3();
	axes = Quaternion();
}

/**************************************************************************************************
* Constructor defining dimensions
**************************************************************************************************/
RigidBody::RigidBody(Vector3 halfWidths, float mass) {
	this->axes = Quaternion();
	this->halfWidths = halfWidths;
	this->mass = mass;

	// Define the moment of inertia tensor
	float w = halfWidths.x;
	float h = halfWidths.z;
	float d = halfWidths.y;

	inertiaTensor = Matrix(3, 3);
	inertiaTensor.matrix[0][0] = (1.0f / 12.0f) * mass * ((h * h) + (d * d));
	inertiaTensor.matrix[1][1] = (1.0f / 12.0f) * mass * ((w * w) + (d * d));
	inertiaTensor.matrix[2][2] = (1.0f / 12.0f) * mass * ((w * w) + (h * h));

	// Calculat the inertia tensor in world coordinates
	worldInertiaTensor = Matrix(3, 3);
	Matrix m = Matrix(3, 3);
	m.matrix[0][0] = 1 - 2*axes.y*axes.y - 2*axes.z*axes.z;
	m.matrix[0][1] = 2*axes.x*axes.y + 2*axes.w*axes.z;
	m.matrix[0][2] = 2*axes.x*axes.z - 2*axes.w*axes.y;

	m.matrix[1][0] = 2*axes.x*axes.y - 2*axes.w*axes.z;
	m.matrix[1][1] = 1 - 2*axes.x*axes.x - 2*axes.z*axes.z;
	m.matrix[1][2] = 2*axes.y*axes.z + 2*axes.w*axes.x;
	
	m.matrix[2][0] = 2*axes.x*axes.z + 2*axes.w*axes.y;
	m.matrix[2][1] = 2*axes.y*axes.z - 2*axes.w*axes.x;
	m.matrix[2][2] = 1 - 2*axes.x*axes.x - 2*axes.y*axes.y;
	
	worldInertiaTensor = (m * inertiaTensor) * m.getTranspose();
}

void RigidBody::init(void) {
	this->axes = Quaternion();
	this->halfWidths = halfWidths;
	this->mass = mass;

	// Define the moment of inertia tensor
	float w = halfWidths.x;
	float h = halfWidths.z;
	float d = halfWidths.y;

	inertiaTensor = Matrix(3, 3);
	inertiaTensor.matrix[0][0] = (1.0f / 12.0f) * mass * ((h * h) + (d * d));
	inertiaTensor.matrix[1][1] = (1.0f / 12.0f) * mass * ((w * w) + (d * d));
	inertiaTensor.matrix[2][2] = (1.0f / 12.0f) * mass * ((w * w) + (h * h));

	// Calculat the inertia tensor in world coordinates
	worldInertiaTensor = Matrix(3, 3);
	Matrix m = Matrix(3, 3);
	m.matrix[0][0] = 1 - 2*axes.y*axes.y - 2*axes.z*axes.z;
	m.matrix[0][1] = 2*axes.x*axes.y + 2*axes.w*axes.z;
	m.matrix[0][2] = 2*axes.x*axes.z - 2*axes.w*axes.y;

	m.matrix[1][0] = 2*axes.x*axes.y - 2*axes.w*axes.z;
	m.matrix[1][1] = 1 - 2*axes.x*axes.x - 2*axes.z*axes.z;
	m.matrix[1][2] = 2*axes.y*axes.z + 2*axes.w*axes.x;
	
	m.matrix[2][0] = 2*axes.x*axes.z + 2*axes.w*axes.y;
	m.matrix[2][1] = 2*axes.y*axes.z - 2*axes.w*axes.x;
	m.matrix[2][2] = 1 - 2*axes.x*axes.x - 2*axes.y*axes.y;
	
	worldInertiaTensor = (m * inertiaTensor) * m.getTranspose();
}

/**************************************************************************************************
* Destructor
**************************************************************************************************/
RigidBody::~RigidBody(void) {
}

/**************************************************************************************************
* Move the rigid body according to it's particularly movement pattern
**************************************************************************************************/
void RigidBody::move(void) {
	center += velocity;
}

/**************************************************************************************************
* Finds the point on the OBB cloest to the specified point
**************************************************************************************************/
void RigidBody::closestPtPointOBB(Vector3 p, Vector3& q) {
	// Calculate the distance between the specified point and the center of the OBB
	Vector3 d = p - center;

	// Start result at center of box, make steps from there
	q = center;

	// Construct axes
	Vector3 u[3] = {Vector3(1.0f, 0, 0), Vector3(0, 1.0f, 0), Vector3(0, 0, 1.0f)};
	u[0].rotate(axes);
	u[1].rotate(axes);
	u[2].rotate(axes);

	// For each obb axis...
	for(int i = 0; i < 3; i++) {
		// ... project d onto that axis to get the distance along the axis of d from the box center
		float dist = d.dot(u[i]);

		// If distance farther than the box extends, clamp to the box
		if(dist > halfWidths[i])
			dist = halfWidths[i];
		else if(dist < -halfWidths[i])
			dist = -halfWidths[i];

		// Step that distance along the axis to get world coordinate
		q += u[i] * dist;
	}

}

/**************************************************************************************************
* Perform moment of inertia calculations
**************************************************************************************************/
void RigidBody::updateForces(float dt) {
	// Find the torques and applied forces
	//****************************************************************************
	// Find R
	Vector3 r = halfWidths;

	// Externally applied force
 	Vector3 appliedTorque = appliedForce.cross(r);
	appliedForce -= appliedTorque;

	// Swap torque and applied force to give more energy to torque, not strictly legal but should product more pleasing results
//	Vector3 temp = appliedTorque;
//	appliedTorque = appliedForce;
//	appliedForce = temp;

	// Sumation of torques
	Vector3 totalTorque = appliedTorque;

	// Update angular momentum
	//****************************************************************************
	angularMomentum.x += totalTorque.x * dt;
	angularMomentum.y += totalTorque.y * dt;
	angularMomentum.z += totalTorque.z * dt;
	// Damping and friction
	//angularMomentum *= spring->dampingConstant;

	// Find the inverse of the moment of inertia tensor
	//****************************************************************************
	Matrix invI = inertiaTensor.getInverse();

	// Find the matrix equivalent of the current rotation quaternion
	//****************************************************************************
	Matrix q = axes.toMatrix();

	// Find q transpose
	//****************************************************************************
	Matrix qT = q.getTranspose();

	// Find the final matrix representative of moment of inertia
	//****************************************************************************
	Matrix moi = (q * invI) * qT;
	// Update angular velocity
	//****************************************************************************
	Quaternion rotW = Quaternion(0.0f, 0.0f, 0.0f, 0.0f);
	rotW.x = (angularMomentum.x * moi.matrix[0][0]) + 
	(angularMomentum.y * moi.matrix[0][1]) + 
	(angularMomentum.z * moi.matrix[0][2]);
	rotW.y = (angularMomentum.x * moi.matrix[1][0]) + 
	(angularMomentum.y * moi.matrix[1][1]) + 
	(angularMomentum.z * moi.matrix[1][2]);
	rotW.z = (angularMomentum.x * moi.matrix[2][0]) + 
	(angularMomentum.y * moi.matrix[2][1]) + 
	(angularMomentum.z * moi.matrix[2][2]);

	aVelocity.x = rotW.x;
	aVelocity.y = rotW.y;
	aVelocity.z = rotW.z;

	// Calculate the quaternion by which to rotate
	//****************************************************************************
	dQ.w = axes.w + dt * ((rotW.w * axes.w) - (rotW.x * axes.x) - (rotW.y * axes.y) - (rotW.z * axes.z)) / 2.0f;
	dQ.x = axes.x + dt * ((rotW.w * axes.x) + (rotW.x * axes.w) + (rotW.y * axes.z) - (rotW.z * axes.y)) / 2.0f;
	dQ.y = axes.y + dt * ((rotW.w * axes.y) - (rotW.x * axes.z) + (rotW.y * axes.w) + (rotW.z * axes.x)) / 2.0f;
	dQ.z = axes.z + dt * ((rotW.w * axes.z) + (rotW.x * axes.y) - (rotW.y * axes.x) + (rotW.z * axes.w)) / 2.0f;
	dQ.normalize(); // Thank you Stuart



	// Ensure that the applied force is limited to one keypress
	appliedForce = Vector3();
}